function []=rrt_run

%     function[] = rect(x,y,l,b)
%       hold on
%       rectangle('Position',[x,y,l,b],'FaceColor',[0 0.5 0.5])
%     end
% 
    function circle(x,y,r)   %画圆函数
        ang=0:0.01:2*pi; 
        xp=r*cos(ang);
        yp=r*sin(ang);
        plot(x+xp,y+yp, '.m');
    end

    function ellipse(x0,y0,a,b)   %画椭圆函数
        % x0,y0是椭圆中心坐标
        % a是长半轴长
        % b是短半轴长
        theta = 0:0.01:2*pi;
        xp = x0 + a * cos(theta);
        yp = y0 + b * sin(theta);
        plot(xp, yp, '.r');
    end

figure;
axis([-2, 10.5, -0.5, 12])
% set(gca, 'XTick', -2:2:10)
% set(gca, 'YTick', 0:2:12)
axis equal
grid on
hold on
% rect(130,70,20,60);   % 绘制障碍物
% rect(70,135,60,20);   % 绘制障碍物
ellipse(5,5,5,5)

p_start = [0;0];    %起点位置
p_goal = [10;10];   %目标位置

param.obstacles =[5,5,5,5;];  %一行代表一个椭圆
param.searchFeild = [-2,10.5,0,12];   %搜索区域
param.threshold = 0.3;   %d_{min}
param.resolution = 10;   %决定在路径上多远取一个点，以判断路径是否避开障碍
param.maxNodes = 800;
param.step_size = 6;
param.neighbourhood = 1; %设置neighbourhood的半径r
param.random_seed = 20;

circle(p_start(1), p_start(2), 0.3)
text(p_start(1)+0.2, p_start(2)+0.2, 'Start');
% plot(p_goal(1), p_goal(2), '.r')
circle(p_goal(1), p_goal(2), 0.3)
text(p_goal(1)+0.2, p_goal(2)+0.2, 'Goal');

result = RRTstar(param, p_start, p_goal)
end